import cv2
import numpy as np
import pyrealsense2 as rs
import mediapipe as mp

# 初始化 Mediapipe 手部模块
mp_hands = mp.solutions.hands
hands = mp_hands.Hands(static_image_mode=False, max_num_hands=2, min_detection_confidence=0.7)
mp_drawing = mp.solutions.drawing_utils

# 配置 RealSense 流
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

# 启动流
pipeline.start(config)

# 定义手指的关键点索引范围
finger_indices = {
    "thumb": range(0, 5),
    "index": range(5, 9),
    "middle": range(9, 13),
    "ring": range(13, 17),
    "pinky": range(17, 21)
}
try:
    while True:
        # 等待获取新的帧
        frames = pipeline.wait_for_frames()
        color_frame = frames.get_color_frame()

        if not color_frame:
            continue

        # 将图像转换为 NumPy 数组
        image = np.asanyarray(color_frame.get_data())

        # 转换颜色空间
        image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
        image_rgb.flags.writeable = False

        # 使用 Mediapipe 检测手部
        results = hands.process(image_rgb)

        # 将图像转换回 BGR 格式
        image_rgb.flags.writeable = True
        annotated_image = cv2.cvtColor(image_rgb, cv2.COLOR_RGB2BGR)

        if results.multi_hand_landmarks:
            for hand_landmarks in results.multi_hand_landmarks:
                mp_drawing.draw_landmarks(annotated_image, hand_landmarks, mp_hands.HAND_CONNECTIONS)
                # 在这里可以调用 Dex-Retargeting 算法，处理 hand_landmarks
                 # 提取并打印每个手指的关键点空间姿态
                for finger, indices in finger_indices.items():
                    keypoints = [(hand_landmarks.landmark[i].x, 
                                  hand_landmarks.landmark[i].y, 
                                  hand_landmarks.landmark[i].z) 
                                 for i in indices]
                    if finger == "thumb":
                        print(f"{finger} keypoints: {keypoints}")

        # 显示结果
        cv2.imshow('Hand Tracking', annotated_image)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

finally:
    # 停止流
    pipeline.stop()
    cv2.destroyAllWindows()

